#include "StdAfx.h"
#include "Graph.h"

namespace Tools
{
	Graph::Graph(void)
	{
		this->node_list.clear();
	}

	Graph::Graph(set< CvPoint,ImagePointComp > list, IplImage *thinned_image)
	{
		set<CvPoint,ImagePointComp>::iterator it;
		for (it = list.begin(); it != list.end(); it++)
		{
			Vertex item;
			item.x = (*it).x;
			item.y = (*it).y;
			this->node_list.push_back(item);
		}
		this->build_Graph(thinned_image);

	}
	
	void Graph::build_Graph(IplImage *thinned_image)
	{
		// For each vertex in graph, build a connection between this vertex and the others
		vector<Vertex>::iterator it;
		set<Vertex,VertexComp> branch;
		for (it = this->node_list.begin(); it != this->node_list.end(); it++)
		{
			branch.clear();
			vector<Vertex> childs;
			childs.clear();
			childs.push_back(*it);
			while(!childs.empty())
			{
				Vertex currentNode = childs.back();
				childs.pop_back();
				branch.insert(currentNode);
			
				if ((*it) != currentNode)
				{
					vector<Vertex>::iterator branch_end;
					branch_end = find(this->node_list.begin(), this->node_list.end(), currentNode);
					if ( branch_end != this->node_list.end())
					{
						Vertex *alink = &(*branch_end);
						(*it).links.insert(alink);
						alink->links.insert(&(*it));
					}
					else
					{
						// Expands current node
						for (int k = -1; k < 2; k++)
						{
							for (int l = -1; l < 2; l++)
							{
								if ((currentNode.y + k < 0) || (currentNode.x + l < 0) || (currentNode.y + k >= thinned_image->height) || (currentNode.x + l >= thinned_image->width) )
								{
									continue;
								}
								else
								{
									if (((uchar *)(thinned_image->imageData + (currentNode.y + l) * thinned_image->widthStep))[currentNode.x + k] == 0)
									{
										Vertex temp;
										temp.x = currentNode.x + k;
										temp.y = currentNode.y + l;
										if (branch.find(temp) == branch.end())
										{
											childs.push_back(temp);
										}
									}
								}
							}
						}

					}
				} 
				else
				{
					// Expands current node
					for (int k = -1; k < 2; k++)
					{
						for (int l = -1; l < 2; l++)
						{
							if ((currentNode.y + k < 0) || (currentNode.x + l < 0) || (currentNode.y + k >= thinned_image->height) || (currentNode.x + l >= thinned_image->width) )
							{
								continue;
							}
							else
							{
								if (((uchar *)(thinned_image->imageData + (currentNode.y + l) * thinned_image->widthStep))[currentNode.x + k] == 0)
								{
									Vertex temp;
									temp.x = currentNode.x + k;
									temp.y = currentNode.y + l;
									if (branch.find(temp) == branch.end())
									{
										childs.push_back(temp);
									}
								}
							}
						}
					}
				}


			}
		}
		return;
	}

	Vertex* Graph::get_vertex(int x, int y)
	{
		vector<Vertex>::iterator it;
		for (it = this->node_list.begin(); it != this->node_list.end(); it++)
		{
			if (((*it).x == x) && ((*it).y == y))
			{
				return &(*it);
			}
		}
		return NULL;
	}

	vector<CvPoint> Graph::get_branch_points(int x1, int y1)
	{
		Vertex *vertex1 = this->get_vertex(x1,y1);
		set<Vertex*>::iterator it;
		vector<CvPoint> retval;
		for (it = vertex1->links.begin(); it != vertex1->links.end(); it++)
		{
			CvPoint pi;
			pi.x = (*it)->x;
			pi.y = (*it)->y;
			retval.push_back(pi);
		}
		return retval;
	}

	vector<CvPoint> Graph::find_neighbours(int x1, int y1)
	{
		Vertex *vertex1 = this->get_vertex(x1,y1);
		vector<CvPoint> neighbours;
		neighbours.clear();

		list<Vertex*> childs;
		set<Vertex*> expandedNodes;
		childs.push_back(vertex1);
		expandedNodes.insert(vertex1);
		while (!childs.empty())
		{
			Vertex *currentNode;
			currentNode = childs.front();
			childs.pop_front();
			set<Vertex*>::iterator it;
			for (it = currentNode->links.begin(); it != currentNode->links.end(); it++)
			{
				if (expandedNodes.find(*it) == expandedNodes.end())
				{
					childs.push_back(*it);
					expandedNodes.insert(*it);
					CvPoint pi;
					pi.x = (*it)->x;
					pi.y = (*it)->y;
					neighbours.push_back(pi);
				}
			}
		}

		return neighbours;
	}

	Graph::~Graph(void)
	{

	}
}